/* gm_sim_physics.c -- Física do simulador
 * 
 * Copyright (C) 2008, Gustavo Campos Martins <gcm.martins@gmail.com>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */


#include <math.h>

#include "gm_sim.h"
#include "gm_sim_utils.h"
#include "gm_sim_physics.h"

extern cpSpace *space;
extern cpBody *robot[NUM_ROB];
extern cpBody *ball;
extern cpBody *staticBody;

extern int kick[NUM_ROB];
extern int dribble[NUM_ROB];
extern int placar_a, placar_b;


/**
 * Inicializa a física do simulador.
 */
void
gm_sim_phys_init( )
{
	/* Inicia o Chipmunk */
	cpInitChipmunk();

	int i;
	for (i = 0; i < NUM_ROB; i++) {
		kick[i] = 0;
		dribble[i] = 0;
	}

	placar_a = 0;
	placar_b = 0;

	/* inicia o espaço */
	space = cpSpaceNew();
	space->gravity = cpv(0.0f, 0.0f);
	
	/* Otimizações */
	//cpSpaceResizeStaticHash(space, 20, 2000);

	/* inicia os robôs */
	float px[10] = { 120.0, 260.0, 380.0, 380.0, 500.0, 
		     1180.0, 1040.0, 920.0, 920.0, 810.0 };
	float py[10] = { 400.0, 400.0, 575.0, 225.0, 400.0, 
		     400.0, 400.0, 575.0, 225.0, 400.0 };
	for (i = 0; i < NUM_ROB; i++) {
		robot[i] = cpBodyNew(INFINITY, INFINITY);
		robot[i]->p = cpv(px[i], py[i]);
		robot[i]->a = 0.0;
		cpSpaceAddBody(space, robot[i]);
	}

	cpVect a, b;
	cpShape *seg;
	for (int j = 0; j < NUM_ROB; ++j) {
		for (int i=35; i<325; i++) {
			float alfa = i*DEG2RAD;
			a = cpv(cos(alfa)*ROBOT_RADIUS, sin(alfa)*ROBOT_RADIUS);
			b = cpv(cos(alfa + 1*DEG2RAD)*ROBOT_RADIUS, sin(alfa + 1*DEG2RAD)*ROBOT_RADIUS);

			seg = cpSegmentShapeNew(robot[j], a, b, 0.0f);
			seg->e = 0.4;
			seg->u = 100.0;
			cpSpaceAddShape(space,seg);
		}
		a = cpv(cos(35*DEG2RAD)*ROBOT_RADIUS, sin(35*DEG2RAD)*ROBOT_RADIUS);
		seg = cpSegmentShapeNew(robot[j], b, a, 0.0f);
		seg->e = 0.4;
		seg->u = 100.0;
		cpSpaceAddShape(space,seg);
	}

	/* inicia a bola */
	ball = cpBodyNew(5000.0f, 0.0f);
	ball->p = cpv(500.0f, 480.0f);
	cpSpaceAddBody(space, ball);

	cpShape *bShape = cpCircleShapeNew(ball, BALL_RADIUS, cpv(0.0f, 0.0f));
	bShape->e = 0.4;
	bShape->u = 100.0;
	cpSpaceAddShape(space, bShape);

	/* inicia as "laterais" do campo */
	staticBody = cpBodyNew(INFINITY, INFINITY);
	seg = cpSegmentShapeNew(staticBody, FIELD_BL, FIELD_BR, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, FIELD_BR, GOALOPP_B, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOPP_B, cpv(WORLD_BR.x, GOALOPP_B.y), 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BR.x, GOALOPP_B.y), cpv(WORLD_BR.x, GOALOPP_U.y), 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BR.x, GOALOPP_U.y), GOALOPP_U, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOPP_U, FIELD_UR, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, FIELD_UR, FIELD_UL, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, FIELD_UL, GOALOUR_U, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOUR_U, cpv(WORLD_BL.x, GOALOUR_U.y) , 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BL.x, GOALOUR_U.y), cpv(WORLD_BL.x, GOALOUR_B.y), 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BL.x, GOALOUR_B.y), GOALOUR_B, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOUR_B, FIELD_BL, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
}

/** Atualiza os dados de *self
 *
 * @param *self Dados do simulador
 */
void
gm_sim_phys_update( )
{
	/* quanto maior for o steps, mais precisos são os cálculos. */
	int steps = 1;
	cpFloat dt = 1.0/60.0/(cpFloat)steps;

	//if (count == 10) {

	//	count = 0;


		/* Se a bola foi chutada */
		for (int i = 0; i < NUM_ROB; i++) {
			if (kick[i]) {
				float dist = sqrt( pow(ball->p.x - (robot[i]->p.x + 
					               cos(robot[i]->a)*ROBOT_RADIUS), 2) +
						   pow(ball->p.y - (robot[i]->p.y +
						       sin(robot[i]->a)*ROBOT_RADIUS), 2) );

				if (dist < 15.0)
					cpBodyApplyImpulse(ball, 
							cpvmult(cpvsub(ball->p, robot[i]->p), 15.0), cpvzero);
				kick[i] = 0;
			}
		}

		/* atrito da bola com o chão */
		/* ball->v = cpvmult(ball->v, 0.995); */

	//}

	//atualiza o Chipmunk
	cpSpaceStep(space, dt);
}

/** Pega a posição da bola.
 *
 * @param *ballpos vetor com duas posições, na primeira posição do vetor será escrita a
 * 		   posição x da bola e  na segunda posição do vetor a posição y da bola.
 */
void
gm_sim_phys_get_ballpos( float * ballpos )
{
	ballpos[0] = ball->p.x;
	ballpos[1] = ball->p.y;
}

/** Pega a velocidade da bola.
 *
 * @param *ballvel vetor com duas posições, na primeira posição do vetor será escrita a
 * 		   velocidade x da bola e  na segunda posição do vetor a velocidade y da bola.
 */
void
gm_sim_phys_get_ballvel( float * ballvel )
{
	ballvel[0] = ball->v.x;
	ballvel[1] = ball->v.y;
}

/** Seta a posição da bola e sua velocidade vai para zero.
 *
 * @param *ballpos nova posição da bola.
 */
void
gm_sim_phys_set_ballpos( float * ballpos )
{
	ball->p.x = ballpos[0];
	ball->p.y = ballpos[1];
	ball->v = cpvzero;
}

/** Seta a velocidade da bola.
 *
 * @param *ballvel nova velocidade da bola.
 */
void
gm_sim_phys_set_ballvel( float * ballvel )
{
	ball->v = cpv(ballvel[0], ballvel[1]);
}

/** Pega a posição do robô.
 *
 * @param *ballpos vetor com duas posições, na primeira posição do vetor será escrita a
 * 		   posição x do robô e  na segunda posição do vetor a posição y do robô.
 *
 * @param i número do robô.
 */
void
gm_sim_phys_get_robotpos( float * robotpos, int i )
{
	robotpos[0] = robot[i]->p.x;
	robotpos[1] = robot[i]->p.y;
}

/** Pega o ângulo do robô.
 *
 * @param *robotang ponteiro para onde será armazenado o ângulo do robô.
 *
 * @param i número do robô.
 */
void
gm_sim_phys_get_robotang( float * robotang, int i )
{
	*robotang = robot[i]->a;
}


/** Seta a posição do robô e sua velocidade vai pra zero.
 *
 * @param *robotpos nova posição do robô.
 *
 * @param i número do robô.
 */
void
gm_sim_phys_set_robotpos( float * robotpos, int i )
{
	robot[i]->p.x = robotpos[0];
	robot[i]->p.y = robotpos[1];
	robot[i]->v = cpvzero;
}
/** Seta a velocidade do robô.
 *
 * @param *robotvel nova velocidade.
 *
 * @param número do robô.
 */
void 
gm_sim_phys_set_robotvel( float * robotvel, int i )
{
	robot[i]->v.x = robotvel[0];
	robot[i]->v.y = robotvel[1];
}

/** Seta o ângulo do robô.
 *
 * @param robotang novo ângulo do robô.
 *
 * @param i número do robô.
 */
void
gm_sim_phys_set_robotw( float robotw, int i )
{
	robot[i]->w = robotw;
}

/** Seta o chute de algum robô
 *
 * @param level o nível (força) de um chute.
 *
 * @param i número do robô.
 */
void
gm_sim_phys_set_kick( int level, int i )
{
	kick[i] = level;
}

/** aciona o rolinho de algum robô
 *
 * @param level se for igual a 0 desativa o rolinho, caso contrário ativa o rolinho
 * 
 * @param i número do robô.
 */
void
gm_sim_phys_set_dribble( int level, int i )
{
	dribble[i] = level;
}
